//
// Created by vivi on 25/04/2018.
//

#include <gtest/gtest.h>
#include "core/util/mathutil.h"
#include "core/util/eigenutil.h"

using namespace std;
using namespace HairEngine;
using namespace Eigen;

TEST(MathUtilTest, TestLineSegmentSquaredDistance) {

	for (size_t _ = 0; _ < 100000; ++_) {
		Vector3f p0 = Vector3f::Random() * 10.0f;
		Vector3f p1 = Vector3f::Random() * 10.0f;
		Vector3f q0 = Vector3f::Random() * 10.0f;
		Vector3f q1 = Vector3f::Random() * 10.0f;

		float s;
		float t;

		float distance = MathUtility::lineSegmentSquaredDistance(p0, p1, q0, q1, s, t);

		Vector3f ps = p0 + (p1 - p0) * s;
		Vector3f qt = q0 + (q1 - q0) * t;

		ostringstream os;
		os << "p0=" << EigenUtility::toString(p0) << " "
		   << "p1=" << EigenUtility::toString(p1) << " "
		   << "q0=" << EigenUtility::toString(q0) << " "
		   << "q1=" << EigenUtility::toString(q1) << " "
		   << "s=" << s << " " << "t=" << t << " " << "distance=" << distance;

		//Test squared norm equality
		float distanceCheck = (qt - ps).squaredNorm();

		if (s < 0.0 || s > 1.0 || t < 0.0 || t > 1.0) {
			MathUtility::lineSegmentSquaredDistance(p0, p1, q0, q1, s, t);
		}
		ASSERT_GE(s, 0.0) << os.str();
		ASSERT_LE(s, 1.0) << os.str();
		ASSERT_GE(t, 0.0) << os.str();
		ASSERT_LE(t, 1.0) << os.str();

		ASSERT_LE(std::abs(distance - distanceCheck), 0.005) << "distanceCheck=" << distanceCheck << " " << os.str();

		//Check minimization
		Vector3f psCheck = ps + (s > 0.5 ? -0.01 : 0.01) * (p1 - p0);
		Vector3f qtCheck = qt + (t > 0.5 ? -0.01 : 0.01) * (q1 - q0);

		ASSERT_LE(distance, (psCheck - qtCheck).squaredNorm())
									<< "psCheck=" << EigenUtility::toString(psCheck) << " "
									<< "qtCheck=" << EigenUtility::toString(qtCheck) << " "
									<< os.str();

		//Check orthology
		if (s > 0.001 && s < 0.998)
			if (t > 0.001f && t < 0.998) {
				float orthoP = (qt - ps).normalized().dot((p1 - p0).normalized());
				float orthoQ = (qt - ps).normalized().dot((q1 - q0).normalized());

				ASSERT_LE(std::abs(orthoP), 0.015) << "orthoP=" << orthoP << " " << os.str();
				ASSERT_LE(std::abs(orthoQ), 0.015) << "orthoQ=" << orthoQ << " " << os.str();
			}
	}
}